/**
 * @file Diag_UDSClient.c
 * @author dowson (you@domain.com)
 * @brief 
 * @version 0.1
 * @date 2023-08-03
 * 
 * @copyright Copyright (c) 2023
 * 
 */
#include "stdint.h"
#include "stdlib.h"
#include "string.h"
#include "Diag_UDSClient.h"
#include "SYS_Disk.h"
#include "system_init.h"
#include "CanIf.h"
#include "at32f403a_407_board.h"

#define CAN_DEBUG 1
#define UDS_3E_WAIT_TIME_MS 40
Diag_BoolType haveRecvData;
static Diag_StateMachineType MainStateMachine = Diag_MIDLE;
extern Diag_UDSClientType UDSClient;
uint8_t UDSKeyValue_0x27[4] = {0};
Diag_UDSClientType *currentUDSClient = &UDSClient;

// static void _udsDelay(uint32_t nms){
// 	uint32_t i=0,j=0;
// 	for(i=0;i<nms;++i){
// 		for(j=3000;j>0;j--){
// 			asm("nop");
// 		}
// 	}
// }

static void Diag_changeState(Diag_StateMachineType state) {
    MainStateMachine = state;
    if (Diag_MWAIT == state) haveRecvData = Diag_FALSE;
}

static Diag_BoolType Diag_HaveRecvData(void){
    return haveRecvData;
}

static Diag_ErrType Diag_PackData(Diag_UDSClientType *client,Diag_DataType dataType,Diag_FrameType frameType,uint8_t sendTimes,Diag_ServiceIdType sid,const uint8_t *srcData,uint16_t srcDataLen,uint8_t *dstBuf,uint8_t dstBufLen){
    uint16_t i = 0;
    uint8_t j = 1;
    memset(dstBuf,0x55,dstBufLen);
    if (SOLO_TYPE == dataType)
    {
        if (srcDataLen > SOLO_DATA_MAX_LEN) return Diag_ERR;
        if (dstBufLen < 8) return Diag_ERR;

        dstBuf[0] = srcDataLen;
        //fprintf(USART2_STREAM,"-SOLO_TYPE:%d,%d,%d,%d,%d,%d,%d\r\n",dstBuf[0],dstBuf[1],dstBuf[2],dstBuf[3],dstBuf[4],dstBuf[5],dstBuf[6]);     
        //fprintf(USART2_STREAM,"-dataType:%d,srcDataLen:%d\r\n",dataType,srcDataLen);
        memcpy(&dstBuf[1],srcData,srcDataLen);
        client->sendedSize += client->sendSize;
    }else{
        if (FIRST_FRAME == frameType)
        {
            srcDataLen &= 0X0fff;
            dstBuf[0] = frameType;
            dstBuf[0] <<= 4;
            dstBuf[0] |= (srcDataLen>>8);
            dstBuf[1] = srcDataLen & 0xff;
            dstBuf[2] = client->sendBuf[0];
            dstBuf[3] = client->sendBuf[1];
            dstBuf[4] = client->sendBuf[2];
            dstBuf[5] = client->sendBuf[3];
            dstBuf[6] = client->sendBuf[4];
            dstBuf[7] = client->sendBuf[5];
            client->sendedSize += 6;
            //fprintf(USART2_STREAM,"-FIRST_FRAME:%d,%d,%d,%d,%d,%d,%d\r\n",dstBuf[0],dstBuf[1],dstBuf[2],dstBuf[3],dstBuf[4],dstBuf[5],dstBuf[6]);     

        }else if (CONTINUE_FRAME == frameType)
        {
            sendTimes &= 0x0f;
            dstBuf[0] = frameType;
            dstBuf[0] <<= 4;
            dstBuf[0] |= sendTimes;
            //fprintf(USART2_STREAM,"-sendedSize:%d,dataSize:%d\r\n",client->sendedSize,client->sendSize);
            for (i = client->sendedSize,j = 1;i < client->sendSize;i++,j++)
            {
                dstBuf[j] = client->sendBuf[i];
                client->sendedSize++;
                if(j >= 7) break;
            }
        }
    }
    return Diag_OK;
}

/**
 * @brief check that the response state
 * 
 * @param data 
 * @param dataLen 
 * @return Diag_ResponseType 
 */
Diag_ResponseType RequestResponseState(Diag_ServiceIdType sId,const uint8_t *data,uint8_t dataLen){
    //fprintf(USART2_STREAM,"-dataLen:%d\r\n",dataLen);     
    if(dataLen<1) return Diag_RESPONSE_ERR;
    //fprintf(USART2_STREAM,"-sid:%x,data[0]:%x,data[1]:%x\r\n",sId,data[0],data[1]);     
    if (0x7f == data[1])
    {
       if (dataLen < 2){
            return Diag_RESPONSE_ERR;
       }else if (0x7f == data[1] && 0x78 == data[3]){
            //表示未准备好，需要再次等待
            return Diag_RESPONSE_RUNNING;
       }else{
            return Diag_RESPONSE_NEG;
       }
    }else{
        if(sId+0x40 != data[1]){
            printf("Diag ERR\r\n");
            return Diag_RESPONSE_ERR;     
        }
        //fprintf(USART2_STREAM,"-Diag_RESPONSE_POS\r\n");     
        return Diag_RESPONSE_POS;
    }
}

Diag_ErrType Diag_RequestCommCallBack(Diag_UDSClientType *client){
    //fprintf(USART2_STREAM,"-timeOutState:%d\r\n",client->RecvDataType.timeOutState);
    if(0x8 == (client->sendBuf[1]>>4))
        {
            client->RecvDataType.responseState = Diag_RESPONSE_POS; 
        }
    else{     
        if(!client->RecvDataType.timeOutState){
            //fprintf(USART2_STREAM,"-recvSize:%d\r\n",client->recvSize);     
            client->RecvDataType.responseState = RequestResponseState(client->sid,client->recvBuf,client->recvSize);
        }else{
            printf("time out\r\n");    
            client->RecvDataType.timeOutState =  Diag_TRUE;
            client->RecvDataType.responseState = Diag_RESPONSE_ERR;
                return Diag_ERR_TIMEOUT;
        }
    }
    return Diag_OK;
}

extern const CanIf_TxPduConfigType CanIf_TxPduConfigData[CANIF_TXPDU_NUMBER];
static void Diag_PollOnceStateMachine(void){
    uint8_t sendCanData[8] = {0};
    Diag_FrameType frameType = SOLO_FRAME;
    Diag_DataType dataType = SOLO_TYPE;

    switch (MainStateMachine)
    {
    case Diag_MIDLE:
        //fprintf(USART2_STREAM,"-Diag_MIDLE\r\n");     
        ;
        break;
    case Diag_MSENDING:
        //fprintf(USART2_STREAM,"-Diag_MSENDING\r\n");  
        if (currentUDSClient->sendSize <= SOLO_DATA_MAX_LEN){
            //TODO:单帧发送数据
            frameType = SOLO_FRAME;
            dataType = SOLO_TYPE;
            Diag_PackData(currentUDSClient,dataType,frameType,currentUDSClient->SendDataType.Sequence,currentUDSClient->sid,currentUDSClient->sendBuf,currentUDSClient->sendSize,sendCanData,8);

            fprintf(USART2_STREAM,"-send can data:[%x][%x][%x][%x][%x][%x][%x][%x]",sendCanData[0],sendCanData[1],sendCanData[2],sendCanData[3],sendCanData[4],sendCanData[5],sendCanData[6],sendCanData[7]);

            if( 0x8 == (currentUDSClient->sendBuf[1]>>4)) 
            {
                Diag_changeState(Diag_MPROCESS);
                currentUDSClient->RecvDataType.timeOutState = Diag_FALSE;
                fprintf(USART2_STREAM,"go8");
            }
            else {
			Diag_changeState(Diag_MWAIT);
			}

            PduInfoType pduInfo;
            pduInfo.MetaDataPtr = NULL;
            pduInfo.SduDataPtr = sendCanData;
            pduInfo.SduLength = 8;
            PduIdType pduId;
            for(int i=0;i<CANIF_TXPDU_NUMBER;++i){
                if(CanIf_TxPduConfigData[i].CanIfTxPduCanId == currentUDSClient->ecuId){
                    pduId = CanIf_TxPduConfigData[i].CanIfTxPduId;
                    break;
                }
            }
            CanIf_Transmit(pduId, &pduInfo);
            // CAN0_Transmit_DATA(CAN0_SFR, currentUDSClient->ecuId, sendCanData, 8, CAN_DATA_FRAME, CAN_FRAME_FORMAT_SFF);   
            //Diag_changeState(Diag_MWAIT);
        }else{
            dataType = MULTI_TYPE;
            if (currentUDSClient->sendedSize == 0){ 
                frameType = FIRST_FRAME;
                currentUDSClient->sendedSize = 0;
                currentUDSClient->SendDataType.FCBlockSendedSize = 0;
                currentUDSClient->SendDataType.Sequence = 0;
                Diag_changeState(Diag_MWAIT);  //等待流控帧
                Diag_PackData(currentUDSClient,dataType,frameType,currentUDSClient->SendDataType.Sequence,currentUDSClient->sid,&currentUDSClient->sendBuf[currentUDSClient->sendedSize],currentUDSClient->sendSize,sendCanData,8);
            }else{
                //fprintf(USART2_STREAM,"-send CONTINUE_FRAME:%d\r\n",currentUDSClient->SendDataType.FCBlockSendedSize);
                frameType = CONTINUE_FRAME;
                //fprintf(USART2_STREAM,"-FCBlock:%d,size:%d,Sequence:%d\r\n",currentUDSClient->SendDataType.FCBlockSendedSize,currentUDSClient->SendDataType.Diag_longDataFCMsg.FCBlockSize,currentUDSClient->SendDataType.Sequence);
                for (;;)
                {
                    //fprintf(USART2_STREAM,"-send Sequence:%d,FCBlockSize:%d\r\n",currentUDSClient->SendDataType.Sequence,currentUDSClient->SendDataType.Diag_longDataFCMsg.FCBlockSize);
                    if(currentUDSClient->SendDataType.Sequence > 0x0f) currentUDSClient->SendDataType.Sequence = 0;
                    Diag_PackData(currentUDSClient,dataType,frameType,currentUDSClient->SendDataType.Sequence,currentUDSClient->sid,&currentUDSClient->sendBuf[currentUDSClient->sendedSize],currentUDSClient->sendSize,sendCanData,8);
                    
                    if(currentUDSClient->SendDataType.FCBlockSendedSize == currentUDSClient->SendDataType.Diag_longDataFCMsg.FCBlockSize){
                        currentUDSClient->SendDataType.FCBlockSendedSize = 0;
                        Diag_changeState(Diag_MWAIT);  //等待流控帧
                        break;
                    }
                    if(currentUDSClient->sendedSize >= currentUDSClient->sendSize){
                        currentUDSClient->sendedSize = 0;
                        Diag_changeState(Diag_MWAIT); //等待肯定响应
                        break;
                    }
                    //防止死循环
                    if(currentUDSClient->SendDataType.FCBlockSendedSize >= 0xff){
                        break;
                    }
                    currentUDSClient->SendDataType.FCBlockSendedSize++;
                    currentUDSClient->SendDataType.Sequence++;
                    //fprintf(USART2_STREAM,"-FCBlock:%d,Sequence:%d\r\n",currentUDSClient->SendDataType.FCBlockSendedSize,currentUDSClient->SendDataType.Sequence);
                    //fprintf(USART2_STREAM,"-send can data:[%x][%x][%x][%x][%x][%x][%x][%x]\r\n",sendCanData[0],sendCanData[1],sendCanData[2],sendCanData[3],sendCanData[4],sendCanData[5],sendCanData[6],sendCanData[7]);
                    delay_ms(2);
                    //delay_ms(currentUDSClient->SendDataType.Diag_longDataFCMsg.sendTimeMin);

                    PduInfoType pduInfo;
                    pduInfo.MetaDataPtr = NULL;
                    pduInfo.SduDataPtr = sendCanData;
                    pduInfo.SduLength = 8;
                    PduIdType pduId;
                    for(int i=0;i<CANIF_TXPDU_NUMBER;++i){
                        if(CanIf_TxPduConfigData[i].CanIfTxPduCanId == currentUDSClient->ecuId){
                            pduId = CanIf_TxPduConfigData[i].CanIfTxPduId;
                            break;
                        }
                    }
                    CanIf_Transmit(pduId, &pduInfo);
                    // CAN0_Transmit_DATA(CAN0_SFR, currentUDSClient->ecuId, sendCanData, 8, CAN_DATA_FRAME, CAN_FRAME_FORMAT_SFF);  
                }
            }
            currentUDSClient->SendDataType.FCBlockSendedSize++;
            currentUDSClient->SendDataType.Sequence++;
            delay_ms(2);
            PduInfoType pduInfo;
            pduInfo.MetaDataPtr = NULL;
            pduInfo.SduDataPtr = sendCanData;
            pduInfo.SduLength = 8;

            fprintf(USART2_STREAM,"-multi send can data:[%x][%x][%x][%x][%x][%x][%x][%x]",sendCanData[0],sendCanData[1],sendCanData[2],sendCanData[3],sendCanData[4],sendCanData[5],sendCanData[6],sendCanData[7]);

            PduIdType pduId;
            for(int i=0;i<CANIF_TXPDU_NUMBER;++i){
                if(CanIf_TxPduConfigData[i].CanIfTxPduCanId == currentUDSClient->ecuId){
                    pduId = CanIf_TxPduConfigData[i].CanIfTxPduId;
                    break;
                }
            }
            CanIf_Transmit(pduId, &pduInfo);
            // CAN0_Transmit_DATA(CAN0_SFR, currentUDSClient->ecuId, sendCanData, 8, CAN_DATA_FRAME, CAN_FRAME_FORMAT_SFF);  
        }
        currentUDSClient->requestTimeMs = 0;
        break;
    case Diag_MWAIT:
        //fprintf(USART2_STREAM,"-Diag_MWAIT\r\n");  
        if(Diag_HaveRecvData()){
            currentUDSClient->RecvDataType.timeOutState = Diag_FALSE;
            Diag_changeState(Diag_MPROCESS);
            //fprintf(USART2_STREAM,"HRD\r\n");  
        }else{
            currentUDSClient->requestTimeMs ++;
            if (currentUDSClient->requestTimeMs > Diag_REQUEST_WAIT_TIME_MAX){
                currentUDSClient->RecvDataType.timeOutState = Diag_TRUE;
                Diag_changeState(Diag_MPROCESS);
                printf("wait time out\r\n");  
            }    
        }
        break;
    case Diag_MPROCESS:
        //fprintf(USART2_STREAM,"-Diag_MPROCESS\r\n");  
        if (!currentUDSClient->RecvDataType.timeOutState){
            //fprintf(USART2_STREAM,"-no timeout\r\n");  
            if ((currentUDSClient->recvBuf[0]>>4) == FLOW_FRAME){
                //fprintf(USART2_STREAM,"-Sequence back\r\n");  
                currentUDSClient->SendDataType.Diag_longDataFCMsg.FCBlockSize = currentUDSClient->recvBuf[1];
                currentUDSClient->SendDataType.Diag_longDataFCMsg.sendTimeMin = currentUDSClient->recvBuf[2];
                Diag_changeState(Diag_MSENDING);
                currentUDSClient->ReqState = Diag_REQ_RUNNING;
            }else{
                if(currentUDSClient->RecvDataType.RecvCallBack){
                    currentUDSClient->RecvDataType.RecvCallBack(&UDSClient);
                }else{
                    Diag_RequestCommCallBack(&UDSClient);
                }
                //如果返回消极相应但是错误码未0x78，说明需要再次等待处理完成的正确返回
                if(Diag_RESPONSE_RUNNING == currentUDSClient->RecvDataType.responseState){
                    currentUDSClient->ReqState = Diag_REQ_RUNNING;
                    Diag_changeState(Diag_MWAIT);
                }else{
                    currentUDSClient->ReqState = Diag_REQ_IDEL;
                    Diag_changeState(Diag_MIDLE);
                }
            }
        }else{
            printf("TIMEOUT\r\n");  
            currentUDSClient->ReqState = Diag_REQ_TIMEOUT;
            if(currentUDSClient->RecvDataType.RecvCallBack)
                currentUDSClient->RecvDataType.RecvCallBack(currentUDSClient);
            else Diag_RequestCommCallBack(currentUDSClient);
            Diag_changeState(Diag_MIDLE);
        }
        currentUDSClient->requestTimeMs = 0;
        break;
    case Diag_MERROR:
        printf("MERROR\r\n");  
        break;
    case Diag_MCOMPLETE:
        printf("MCOMPLETE\r\n");  
        Diag_changeState(Diag_MIDLE);
        break;    
    case Diag_MSTOP:
        printf("MSTOP\r\n");  
        break; 
    default:
        Diag_changeState(Diag_MIDLE);
        break;
    }    
}

void Diag_ClearRequestContext(Diag_UDSClientType *client) {
    client->ReqState = Diag_REQ_IDEL;
    memset(client->recvBuf,0xaa,UDS_RECV_BUFSIZE);
    client->RecvDataType.RecvCallBack = NULL;
    client->recvSize = 0;
    client->requestTimeMs = 0;
    client->maxTimeOutMs = Diag_REQUEST_WAIT_TIME_MAX;
    client->sendSize = 0;
    memset(client->sendBuf,0x55,UDS_SEND_BUFSIZE);
    client->TpHandle = NULL;
    client->ReqState = Diag_REQ_IDEL;
    client->RecvDataType.responseState =Diag_RESPONSE_RUNNING;
}



void Diag_CanIqrRecvCallBack(Diag_UDSClientType *client,uint32_t canId,uint8_t canData[8],uint8_t recvSize){

#if CAN_DEBUG
    printf("Diag_CanIqrRecvCallBack-canData(HEX FORMAT):\r\n");
    for(int i=0;i<recvSize;++i){
        printf(" %x ", canData[i]);
    }
    printf("\r\n");
    printf("Diag_CanIqrRecvCallBack-haveRecvData:%d\r\n", haveRecvData);
    printf("-client->responseId:0x%x, canId:0x%x, recvSize:%d\r\n",client->responseId, canId, recvSize);   
#endif  

    if (client->responseId == canId && Diag_FALSE == haveRecvData)
    {
        if(recvSize>8)return;
        if((0x7f == canData[1]) && (0x78 == canData[2]))return;
        memcpy(client->recvBuf,canData,recvSize);
        client->recvSize = recvSize;
        //fprintf(USART2_STREAM,"-client->recvSize:%d\r\n",client->recvSize);

        if(canData[0] == 3 && canData[1] == 0x7f && canData[2] == 0x31 && canData[3] == 0x78){
            // 03 7f 31 78表示需要等待
        }else{
            haveRecvData = Diag_TRUE;
        }
    }else{
        //TODO:回传不是发送请求的数据，可能是心跳也可能是其他数据，需要单独处理
    }
}

Diag_ErrType Diag_RequestSend(Diag_UDSClientType *client){
    //fprintf(USART2_STREAM,"-Diag_RequestSend\r\n");       
    if(0 == client->sendSize) return Diag_ERR_INVALID_ARG;
    if(0 == client->sid) return Diag_ERR_DID_MISMATCH;
    if(Diag_MIDLE != MainStateMachine) return Diag_ERR_BUSY;
    
    client->sendedSize = 0;
    client->recvedSize = 0;
    client->requestTimeMs = 0;
    client->ReqState = Diag_REQ_RUNNING;
    client->SendDataType.Sequence = 0;
    UDSClient.RecvDataType.responseState =Diag_RESPONSE_RUNNING;
    memset(client->recvBuf,0x00,UDS_RECV_BUFSIZE);
    Diag_changeState(Diag_MSENDING);
    Diag_PollOnceStateMachine();
    return Diag_OK;
}


/**
 * @brief UDS client main loop
 * 
 */
void Diag_RequestHandleServiceMainLoop(void){
    Diag_PollOnceStateMachine();
}

//--------------------------------------squence send function--------------------------------

Diag_ErrType Diag_RequestFuncHandle(Diag_SquenceUDSListType *Diag_SquenceUDSList,Diag_SquenceUDSType *squenceNode){
    Diag_ClearRequestContext(Diag_SquenceUDSList->client);
    Diag_SquenceUDSList->client->RecvDataType.RecvCallBack = squenceNode->CallBack;
    if(Diag_FUNC_ID == squenceNode->FuncIdMark) Diag_SquenceUDSList->client->ecuId = 0x7df;
    else Diag_SquenceUDSList->client->ecuId = Diag_SquenceUDSList->phyId;
    Diag_SquenceUDSList->client->responseId = Diag_SquenceUDSList->responseId;
    Diag_SquenceUDSList->client->ecuType = Diag_SquenceUDSList->ecuType;

    if(Diag_0X11_SendEcuResetFunc == squenceNode->Diag_RequestServiceIDFunc){
        Diag_SquenceUDSList->client->sid = Diag_SID_ECU_RESET;
        return Diag_0X11_SendEcuReset(Diag_SquenceUDSList->client,(UDSECUReset_t)squenceNode->paramList[0]);
    }
    if(Diag_0X10_SendDiagSessCtrlFunc == squenceNode->Diag_RequestServiceIDFunc){
        Diag_SquenceUDSList->client->sid = Diag_SID_DIAGNOSTIC_SESSION_CONTROL;
        return Diag_0X10_SendDiagSessCtrl(Diag_SquenceUDSList->client,(Diag_UDSSessionType)squenceNode->paramList[0]);
    }
    if(Diag_0X28_SendCommCtrlFunc == squenceNode->Diag_RequestServiceIDFunc){
        Diag_SquenceUDSList->client->sid = Diag_SID_COMMUNICATION_CONTROL;
        return Diag_0X28_SendCommCtrl(Diag_SquenceUDSList->client,(Diag_UDSCommunicationControlType)squenceNode->paramList[0],\
            (Diag_UDSCommunicationType)squenceNode->paramList[1]);
    }
    if(Diag_0X31_SendRoutineCtrlFunc == squenceNode->Diag_RequestServiceIDFunc){
        Diag_SquenceUDSList->client->sid = Diag_SID_ROUTINE_CONTROL;
        return Diag_0X31_SendRoutineCtrl(Diag_SquenceUDSList->client,(Diag_RoutineControlType)squenceNode->paramList[0],\
        (uint16_t)squenceNode->paramList[1],(const uint8_t *)squenceNode->paramList[2],(uint16_t)squenceNode->paramList[3]);

    }
    if(Diag_0X34_SendRequestDownloadFunc == squenceNode->Diag_RequestServiceIDFunc){
        Diag_SquenceUDSList->client->sid = Diag_SID_REQUEST_DOWNLOAD;
        return Diag_0X34_SendRequestDownload(Diag_SquenceUDSList->client,(uint8_t)squenceNode->paramList[0],(uint8_t)squenceNode->paramList[1],\
        (size_t)squenceNode->paramList[2],(size_t)squenceNode->paramList[3]);
    }
    if(Diag_0X36_SendTransferDataFunc == squenceNode->Diag_RequestServiceIDFunc){
        Diag_SquenceUDSList->client->sid = Diag_SID_TRANSFER_DATA;
        return Diag_0X36_SendTransferData(Diag_SquenceUDSList->client,(uint8_t)squenceNode->paramList[0],(const uint16_t)squenceNode->paramList[1]\
        ,(const uint8_t *)squenceNode->paramList[2],(uint16_t)squenceNode->paramList[3]);
    }
    if(Diag_0X37_SendRequestTransferExitFunc == squenceNode->Diag_RequestServiceIDFunc){
        Diag_SquenceUDSList->client->sid = Diag_SID_REQUEST_TRANSFER_EXIT;
        return Diag_0X37_SendRequestTransferExit(Diag_SquenceUDSList->client);
    }
    if(Diag_0X27_SendSecurityAccessFunc == squenceNode->Diag_RequestServiceIDFunc){
        Diag_SquenceUDSList->client->sid = Diag_SID_SECURITY_ACCESS;
        return Diag_0X27_SendSecurityAccess(Diag_SquenceUDSList->client,(uint8_t)squenceNode->paramList[0],\
        (const uint8_t *)squenceNode->paramList[1],(uint16_t)squenceNode->paramList[2]);
    }
    if(Diag_0X22_SendRDBIFunc == squenceNode->Diag_RequestServiceIDFunc){
        Diag_SquenceUDSList->client->sid = Diag_SID_READ_DATA_BY_IDENTIFIER;
        return Diag_0X22_SendRDBI(Diag_SquenceUDSList->client,(const uint16_t *)squenceNode->paramList[0],\
        (uint16_t)squenceNode->paramList[1]);
    }
    if(Diag_0X2E_SendWDBIFunc == squenceNode->Diag_RequestServiceIDFunc){
        Diag_SquenceUDSList->client->sid = Diag_SID_WRITE_DATA_BY_IDENTIFIER;
        return Diag_0X2E_SendWDBI(Diag_SquenceUDSList->client,(uint16_t)squenceNode->paramList[0],\
        (const uint8_t *)squenceNode->paramList[1],(uint16_t)squenceNode->paramList[2]);  // ybwdebug (const uint16_t *)squenceNode->paramList[1]
    }
    if(Diag_0X3E_SendTesterPresentFunc == squenceNode->Diag_RequestServiceIDFunc){
        Diag_SquenceUDSList->client->sid = Diag_SID_TESTER_PRESENT;
        return Diag_0X3E_SendTesterPresent(Diag_SquenceUDSList->client);
    }
    if(Diag_0X85_DTCControlFunc == squenceNode->Diag_RequestServiceIDFunc){
        Diag_SquenceUDSList->client->sid = Diag_SID_CONTROL_DTC_SETTING;
        return Diag_0X85_DTCControl(Diag_SquenceUDSList->client,(Diag_DTCModeType)squenceNode->paramList[0]);
    }
    fprintf(USART2_STREAM,"no service func:%d",squenceNode->Diag_RequestServiceIDFunc);
    return Diag_ERR;
};

/**
 * @brief 复位序列的状态方便重新发送
 * 
 * @param Diag_SquenceUDSList 
 * @param beginItem  需要初始化的起始节点
 */
Diag_ErrType Diag_UDSSquenceListDefaultReset(Diag_SquenceUDSListType *Diag_SquenceUDSList,uint8_t beginItem){
    uint16_t i = 0;
    if(beginItem >= Diag_SquenceUDSList->listSize)return Diag_ERR;
    Diag_SquenceUDSList->currentItem = beginItem;
    Diag_SquenceUDSList->ret = Diag_OK;
    for (i = 0; i < Diag_SquenceUDSList->listSize; i++)
    {
        Diag_SquenceUDSList->SquenceNodeListHead[i].nodeState = Diag_REQ_IDEL;
    }
    return Diag_OK;
}

/**
 * @brief UDS squence send function to use send multi diagnose request,and auto handle
 * 
 * @param Diag_SquenceUDSList 
 * @param listSize 
 * @return Diag_SequenceErrType 
 */
Diag_SquenceFuncWorkingStateType Diag_UDSSquenceDoing(Diag_SquenceUDSListType *Diag_SquenceUDSList){
    Diag_ErrType ret = Diag_ERR;
    Diag_ResponseType responseState = Diag_RESPONSE_ERR;
    // Diag_SquenceFuncWorkingStateType stateRet = Diag_SQUENCE_NONE;
    static uint8_t delayTimes = 0;

    //if(Diag_SquenceUDSList->currentItem > Diag_SquenceUDSList->listSize) return Diag_SQUENCE_FAIL;

    switch (Diag_SquenceUDSList->SquenceNodeListHead[Diag_SquenceUDSList->currentItem].nodeState)
    {
    case Diag_REQ_IDEL:
        delayTimes = 0;
        ret = Diag_RequestFuncHandle(Diag_SquenceUDSList,&Diag_SquenceUDSList->SquenceNodeListHead[Diag_SquenceUDSList->currentItem]);
        if (Diag_OK == ret)
        {
            //发送成功，去等待结果
            //fprintf(USART2_STREAM,"go running item:%d,sendID:%x,rqsID:%x,list:%d\r\n",Diag_SquenceUDSList->currentItem,Diag_SquenceUDSList->phyId,Diag_SquenceUDSList->responseId,Diag_SquenceUDSList->listSize);
            //如果发送为0x3e 会话保持，则应该延迟一段时间，以至于不会平凡发送（临时方法）
            if (Diag_0X3E_SendTesterPresentFunc == Diag_SquenceUDSList->SquenceNodeListHead[Diag_SquenceUDSList->currentItem].Diag_RequestServiceIDFunc){
                delayTimes = UDS_3E_WAIT_TIME_MS/TimeSlice_GetSliceTime();
                Diag_SquenceUDSList->SquenceNodeListHead[Diag_SquenceUDSList->currentItem].nodeState = Diag_REQ_DELAY;
            }else{
                delayTimes = 0;
                Diag_SquenceUDSList->SquenceNodeListHead[Diag_SquenceUDSList->currentItem].nodeState = Diag_REQ_RUNNING;
            }
        }else if(Diag_ERR_BUSY == ret){
            //TODO:增加忙超时机制
        }else{
            Diag_SquenceUDSList->ret = ret;
            return Diag_SQUENCE_FAIL;
        }
        break;
    case Diag_REQ_RUNNING:
        //fprintf(USART2_STREAM,"RUN\r\n");
        responseState = Diag_SquenceUDSList->client->RecvDataType.responseState;
        if (Diag_RESPONSE_POS == responseState)
        {
            Diag_SquenceUDSList->SquenceNodeListHead[Diag_SquenceUDSList->currentItem].nodeState = Diag_REQ_IDEL;
            Diag_SquenceUDSList->currentItem ++;
            //fprintf(USART2_STREAM,"currentItem:%d,listSize:%d\r\n",Diag_SquenceUDSList->currentItem,Diag_SquenceUDSList->listSize);
            if ((Diag_SquenceUDSList->currentItem) >= (Diag_SquenceUDSList->listSize))
            {
                //fprintf(USART2_STREAM,"SQUENCE_SUCCEED-currentItem:%d,listSize:%d\r\n",Diag_SquenceUDSList->currentItem,Diag_SquenceUDSList->listSize);
                return Diag_SQUENCE_SUCCEED;
            }else{
                Diag_SquenceUDSList->SquenceNodeListHead[Diag_SquenceUDSList->currentItem].nodeState = Diag_REQ_IDEL;
            }
        }else if(Diag_RESPONSE_RUNNING == responseState){
            //继续执行
            return Diag_SQUENCE_WORKING;
        }else{
            if((Diag_0X28_SendCommCtrlFunc == Diag_SquenceUDSList->SquenceNodeListHead[Diag_SquenceUDSList->currentItem].Diag_RequestServiceIDFunc)
            || (Diag_0X85_DTCControlFunc == Diag_SquenceUDSList->SquenceNodeListHead[Diag_SquenceUDSList->currentItem].Diag_RequestServiceIDFunc))
            {
            Diag_SquenceUDSList->SquenceNodeListHead[Diag_SquenceUDSList->currentItem].nodeState = Diag_REQ_IDEL;
                Diag_SquenceUDSList->currentItem ++;
            #ifdef NEED_LOG_UDS_CLIENT
                fprintf(USART2_STREAM,"currentItem:%d,listSize:%d\r\n",Diag_SquenceUDSList->currentItem,Diag_SquenceUDSList->listSize);
            #endif
                if ((Diag_SquenceUDSList->currentItem) >= (Diag_SquenceUDSList->listSize))
                {
                #ifdef NEED_LOG_UDS_CLIENT
                    fprintf(USART2_STREAM,"SQUENCE_SUCCEED-currentItem:%d,listSize:%d\r\n",Diag_SquenceUDSList->currentItem,Diag_SquenceUDSList->listSize);
                #endif
                    return Diag_SQUENCE_SUCCEED;
                }else{
                    Diag_SquenceUDSList->SquenceNodeListHead[Diag_SquenceUDSList->currentItem].nodeState = Diag_REQ_IDEL;
                }
            }
            else
            {
                return Diag_SQUENCE_FAIL;
            }
        }
        break;   
    case Diag_REQ_DELAY:
        if (!delayTimes)
            Diag_SquenceUDSList->SquenceNodeListHead[Diag_SquenceUDSList->currentItem].nodeState = Diag_REQ_RUNNING;     
        delayTimes --;
    break;  
    default:
        break;
    }
    return Diag_SQUENCE_WORKING;
}

/*FUNCTION**********************************************************************
 *
 * Function Name :KeyCalculate
 * Description   :安全访问算法
 * import		 :Seed     种子
 * import		 :ECU_MASK 安全访问权限掩码 , forTemp:  32:AC8   35:AC7
 * return 		 :密钥
 *END**************************************************************************/
static uint32_t Diag_KeyCalculate( uint32_t Seed, uint32_t ECU_MASK,uint8_t loopTimes)
{
	uint32_t Key = 0;
	uint8_t   i;

	if(Seed != 0)
	{
		for(i=0; i<loopTimes; i++)
		{
			if(Seed & 0x80000000)
			{
				Seed = Seed<<1;
				Seed = Seed^ECU_MASK;
			}
			else
			{
				Seed = Seed<<1;
			}
		}
		Key = Seed;
	}
	return Key;
}



//密钥计算
uint32_t Diag_KeyComputation(uint32_t Seed,Diag_ECU_TYPE ecu,uint8_t loopTimes)
{
    uint32_t DUS_ECU_MARSK = 0;
    if(ecu >= UDS_UNKNOW) return 0;
    switch (ecu)
    {
    case UDS_VCU:
        DUS_ECU_MARSK = 0x31564655;
        break;
    case UDS_MCU:
        DUS_ECU_MARSK = 0x41532553;
        break;
    case UDS_TBOX:
        DUS_ECU_MARSK = 0x48414441;
        break;  
    case UDS_BMS:
        DUS_ECU_MARSK = 0x54323251;//原始0x37455554;
        break;
    case UDS_OBCD:
        DUS_ECU_MARSK = 0x3A4D4355;
        break;
    case UDS_OBC:
        DUS_ECU_MARSK = 0x334D4320;
        break;
    case UDS_DCDC1:
        DUS_ECU_MARSK = 0x47455442;
        break;  
    case UDS_DCDC2:
        DUS_ECU_MARSK = 0x47367753;
        break;
    case UDS_IP:
        DUS_ECU_MARSK = 0x35443344;
        break;
    case UDS_ABS:
        DUS_ECU_MARSK = 0x344D4342;
        break;
    case UDS_AVAS:
        DUS_ECU_MARSK = 0x564D4352;
        break;
    case UDS_EPS:
        DUS_ECU_MARSK = 0x33424D54;
        break;
    case UDS_EPB:
        DUS_ECU_MARSK = 0x36464C52;
        break;
    case UDS_EHB:
        DUS_ECU_MARSK = 0x36464C52;
        break;
    case UDS_BCM:
        DUS_ECU_MARSK = 0x36464C52;
        break;
    case UDS_AC:
        DUS_ECU_MARSK = 0x36464C52;
        break;
    case UDS_TPMS:
        DUS_ECU_MARSK = 0x36464C52;
        break;
    case UDS_SRS:
        DUS_ECU_MARSK = 0x36464C52;
        break;
    default:
        DUS_ECU_MARSK = 0X00;
        break;
    }
	return Diag_KeyCalculate(Seed,DUS_ECU_MARSK,loopTimes);
}



//--------------------------------UDS server id funciton----------------------------------

#define PRE_REQUEST_CHECK()                                                                        \
    if (Diag_MIDLE != MainStateMachine) {                                                      \
        return Diag_ERR_BUSY;                                                                       \
    }                                                                                              


/**
 * @brief 0x11
 * 
 * @param client 
 * @param type 
 * @return Diag_ErrType 
 */
Diag_ErrType Diag_0X11_SendEcuReset(Diag_UDSClientType *client,UDSECUReset_t type){
    PRE_REQUEST_CHECK();
    client->sendBuf[0] = Diag_SID_ECU_RESET;
    client->sendBuf[1] = type;
    client->sendSize = 2;
    return Diag_RequestSend(client);
}
/**
 * @brief 0x10
 * 
 * @param client 
 * @param mode 
 * @return Diag_ErrType 
 */
Diag_ErrType Diag_0X10_SendDiagSessCtrl(Diag_UDSClientType *client, Diag_UDSSessionType mode) {
    PRE_REQUEST_CHECK();
    client->sendBuf[0] = Diag_SID_DIAGNOSTIC_SESSION_CONTROL;
    client->sendBuf[1] = mode;
    client->sendSize = 2;
    return Diag_RequestSend(client);
}
/**
 * @brief 0x28
 * 
 * @param client 
 * @param ctrl 
 * @param comm 
 * @return Diag_ErrType 
 */
Diag_ErrType Diag_0X28_SendCommCtrl(Diag_UDSClientType *client, Diag_UDSCommunicationControlType ctrl,Diag_UDSCommunicationType comm) {
    PRE_REQUEST_CHECK();
    client->sendBuf[0] = Diag_SID_COMMUNICATION_CONTROL;
    client->sendBuf[1] = ctrl;
    client->sendBuf[2] = comm;
    client->sendSize = 3;
    return Diag_RequestSend(client);
}
/**
 * @brief 0x3e
 * 
 * @param client 
 * @return Diag_ErrType 
 */
Diag_ErrType Diag_0X3E_SendTesterPresent(Diag_UDSClientType *client) {
    PRE_REQUEST_CHECK();
    client->sendBuf[0] = Diag_SID_TESTER_PRESENT;
    client->sendBuf[1] = 0;
    client->sendSize = 2;
    return Diag_RequestSend(client);
}
/**
 * @brief 0x22
 * 
 * @param client 
 * @param didList 
 * @param numDataIdentifiers 
 * @return Diag_ErrType 
 */
Diag_ErrType Diag_0X22_SendRDBI(Diag_UDSClientType *client, const uint16_t *didList,
                     const uint16_t numDataIdentifiers) {
    PRE_REQUEST_CHECK();
    client->sendBuf[0] = Diag_SID_READ_DATA_BY_IDENTIFIER;
    for (int i = 0; i < numDataIdentifiers; i++) {
        uint16_t offset = 1 + sizeof(uint16_t) * i;
        if (offset + 2 > UDS_SEND_BUFSIZE) {
            return Diag_ERR_INVALID_ARG;
        }
        (client->sendBuf + offset)[0] = (didList[i] & 0xFF00) >> 8;
        (client->sendBuf + offset)[1] = (didList[i] & 0xFF);
    }
    client->sendSize = 1 + (numDataIdentifiers * sizeof(uint16_t));
    return Diag_RequestSend(client);
}

/**
 * @brief 0x2e
 * 
 * @param client 
 * @param dataIdentifier 
 * @param data 
 * @param size 
 * @return Diag_ErrType 
 */
Diag_ErrType Diag_0X2E_SendWDBI(Diag_UDSClientType *client, uint16_t dataIdentifier, const uint8_t *data,
                     uint16_t size) {
    PRE_REQUEST_CHECK();
    client->sendBuf[0] = Diag_SID_WRITE_DATA_BY_IDENTIFIER;
    if (UDS_SEND_BUFSIZE <= 3 || size > UDS_SEND_BUFSIZE - 3) {
        return Diag_ERR_BUFSIZ;
    }
    client->sendBuf[1] = (dataIdentifier & 0xFF00) >> 8;
    client->sendBuf[2] = (dataIdentifier & 0xFF);
    memmove(&client->sendBuf[3], data, size);
    client->sendSize = 3 + size;
    return Diag_RequestSend(client);
}

/**
 * @brief 0x31
 * 
 * @param client 
 * @param type 
 * @param routineIdentifier 
 * @param data 
 * @param size 
 * @return Diag_ErrType 
 */
Diag_ErrType Diag_0X31_SendRoutineCtrl(Diag_UDSClientType *client, Diag_RoutineControlType type,
                            uint16_t routineIdentifier, const uint8_t *data, uint16_t size) {
    PRE_REQUEST_CHECK();
    client->sendBuf[0] = Diag_SID_ROUTINE_CONTROL;
    client->sendBuf[1] = type;
    client->sendBuf[2] = routineIdentifier >> 8;
    client->sendBuf[3] = routineIdentifier;
    if (size) {
        if (size > UDS_SEND_BUFSIZE - Diag_0X31_REQ_MIN_LEN) {
            return Diag_ERR_BUFSIZ;
        }
        memmove(&client->sendBuf[Diag_0X31_REQ_MIN_LEN], data, size);
    } else {
        //TODO:不发送数据
        client->sendBuf[4] = 0x00;
        client->sendBuf[5] = 0x00;
        client->sendBuf[6] = 0x00;
        client->sendBuf[7] = 0x00;
    }
    client->sendSize = Diag_0X31_REQ_MIN_LEN + size;
    // fprintf(USART2_STREAM,"-0x31sid-%d:[%x],[%x],[%x],[%x],[%x],[%x],[%x],[%x]--\r\n",\
    //             client->sendSize,client->sendBuf[0],client->sendBuf[1],client->sendBuf[2],\
    //             client->sendBuf[3],client->sendBuf[4],client->sendBuf[5],client->sendBuf[7]);
    return Diag_RequestSend(client);
}

/**
 * @brief 0x34
 * 
 * @param client 
 * @param dataFormatIdentifier 
 * @param addressAndLengthFormatIdentifier 
 * @param memoryAddress 
 * @param memorySize 
 * @return UDSErr_t 
 */
Diag_ErrType Diag_0X34_SendRequestDownload(Diag_UDSClientType *client, uint8_t dataFormatIdentifier,
                                uint8_t addressAndLengthFormatIdentifier, size_t memoryAddress,
                                size_t memorySize) {
    PRE_REQUEST_CHECK();
    uint8_t numMemorySizeBytes = (addressAndLengthFormatIdentifier & 0xF0) >> 4;
    uint8_t numMemoryAddressBytes = addressAndLengthFormatIdentifier & 0x0F;

    client->sendBuf[0] = Diag_SID_REQUEST_DOWNLOAD;
    client->sendBuf[1] = dataFormatIdentifier;
    client->sendBuf[2] = addressAndLengthFormatIdentifier;

    uint8_t *ptr = &client->sendBuf[Diag_0X34_REQ_BASE_LEN];

    for (int i = numMemoryAddressBytes - 1; i >= 0; i--) {
        *ptr = (memoryAddress & (0xFF << (8 * i))) >> (8 * i);
        ptr++;
    }

    for (int i = numMemorySizeBytes - 1; i >= 0; i--) {
        *ptr = (memorySize & (0xFF << (8 * i))) >> (8 * i);
        ptr++;
    }

    client->sendSize = Diag_0X34_REQ_BASE_LEN + numMemoryAddressBytes + numMemorySizeBytes;
    return Diag_RequestSend(client);
}

/**
 * @brief 0x35
 * 
 * @param client 
 * @param dataFormatIdentifier 
 * @param addressAndLengthFormatIdentifier 
 * @param memoryAddress 
 * @param memorySize 
 * @return UDSErr_t 
 */
Diag_ErrType Diag_0X35_SendRequestUpload(Diag_UDSClientType *client, uint8_t dataFormatIdentifier,
                              uint8_t addressAndLengthFormatIdentifier, size_t memoryAddress,
                              size_t memorySize) {
    PRE_REQUEST_CHECK();
    uint8_t numMemorySizeBytes = (addressAndLengthFormatIdentifier & 0xF0) >> 4;
    uint8_t numMemoryAddressBytes = addressAndLengthFormatIdentifier & 0x0F;

    client->sendBuf[0] = Diag_SID_REQUEST_UPLOAD;
    client->sendBuf[1] = dataFormatIdentifier;
    client->sendBuf[2] = addressAndLengthFormatIdentifier;

    uint8_t *ptr = &client->sendBuf[Diag_0X35_REQ_BASE_LEN];

    for (int i = numMemoryAddressBytes - 1; i >= 0; i--) {
        *ptr = (memoryAddress & (0xFF << (8 * i))) >> (8 * i);
        ptr++;
    }

    for (int i = numMemorySizeBytes - 1; i >= 0; i--) {
        *ptr = (memorySize & (0xFF << (8 * i))) >> (8 * i);
        ptr++;
    }

    client->sendSize = Diag_0X35_REQ_BASE_LEN + numMemoryAddressBytes + numMemorySizeBytes;
    return Diag_RequestSend(client);
}

/**
 * @brief 0x36
 * 
 * @param client 
 * @param blockSequenceCounter 
 * @param blockLength 
 * @param data 
 * @param size 
 * @return UDSErr_t 
 */
Diag_ErrType Diag_0X36_SendTransferData(Diag_UDSClientType *client, uint8_t blockSequenceCounter,
                             const uint16_t blockLength, const uint8_t *data, uint16_t size) {
    PRE_REQUEST_CHECK();
//    assert(blockLength > 2);         // blockLength must include SID and sequenceCounter
//    assert(size + 2 <= blockLength); // data must fit inside blockLength - 2
    client->sendBuf[0] = Diag_SID_TRANSFER_DATA;
    client->sendBuf[1] = blockSequenceCounter;
    memmove(&client->sendBuf[Diag_0X36_REQ_BASE_LEN], data, size);
    client->sendSize = Diag_0X36_REQ_BASE_LEN + size;
    return Diag_RequestSend(client);
}

Diag_ErrType Diag_0X36_SendTransferDataStream(Diag_UDSClientType *client, uint8_t blockSequenceCounter,
                                   const uint16_t blockLength, tFileHead *fd) {
    PRE_REQUEST_CHECK();
//    assert(blockLength > 2); // blockLength must include SID and sequenceCounter
//    client->sendBuf[0] = Diag_SID_TRANSFER_DATA;
//    client->sendBuf[1] = blockSequenceCounter;
//
//    uint16_t size = fread(&client->sendBuf[2], 1, blockLength - 2, fd);
//    client->sendSize = Diag_0X36_REQ_BASE_LEN + size;
    return Diag_RequestSend(client);
}

/**
 * @brief 0x37
 * 
 * @param client 
 * @return UDSErr_t 
 */
Diag_ErrType Diag_0X37_SendRequestTransferExit(Diag_UDSClientType *client) {
    PRE_REQUEST_CHECK();
    client->sendBuf[0] = Diag_SID_REQUEST_TRANSFER_EXIT;
    client->sendSize = 1;
    return Diag_RequestSend(client);
}

/**
 * @brief 0x27
 * 
 * @param client 
 * @param level 
 * @param data 
 * @param size 
 * @return UDSErr_t 
 */
Diag_ErrType Diag_0X27_SendSecurityAccess(Diag_UDSClientType *client, uint8_t level, const uint8_t *data, uint16_t size) {
    PRE_REQUEST_CHECK();
    client->sendBuf[0] = Diag_SID_SECURITY_ACCESS;
    client->sendBuf[1] = level;
    if (size) {
        if (size > UDS_SEND_BUFSIZE - Diag_0X27_REQ_BASE_LEN) {
            return Diag_ERR_BUFSIZ;
        }
    } else {
//        assert(NULL == data);
    }

    memmove(&client->sendBuf[Diag_0X27_REQ_BASE_LEN], data, size);
    client->sendSize = Diag_0X27_REQ_BASE_LEN + size;
    return Diag_RequestSend(client);
}
/**
 * @brief 0x85
 * 
 * @param client 
 * @return Diag_ErrType 
 */
Diag_ErrType Diag_0X85_DTCControl(Diag_UDSClientType *client,Diag_DTCModeType mode) {
    PRE_REQUEST_CHECK();
    client->sendBuf[0] = Diag_SID_CONTROL_DTC_SETTING;
    client->sendBuf[1] = mode;
    client->sendSize = 2;
    return Diag_RequestSend(client);
}







